/**
 *=============================================================================
 *  @copyright (C)      2025CCUT_1024lab_Damocles
 *
 *  @file    template.c
 *  @brief   封装模块代码规范
 *
 *  @note    使用方法
 *
 *
 *  @attention 注意事项
 *-----------------------------------------------------------------------------
 *  @version    @author    @date    @note
 *  V1.0.0      Cherno    250808    done
 *
 *==============================================================================
 */

/*==============================Includes======================================*/
#include <stdarg.h>
#include <stdio.h>
#include <string.h>

#include "dvc_serialplot.h"
/*==============================PrivateMacros=================================*/

/*==============================PrivateTypes==================================*/

/*==============================PrivateVariables==============================*/

/*==============================PrivateFunctionDeclarations===================*/

/*==============================FunctionPrototypes============================*/

/**
 * @brief : 描述该函数主要功能
 * @note : 注意事项
 *
 * @param : 参数名（与函数里的参数一致）  参数的描述
 * @return : 返回值的说明
 */

static float bytes_to_float(const uint8_t bytes[], uint8_t offset)
{
	float value;
	const uint8_t *src = bytes + offset;
	memcpy(&value, src, 4);
	return value;
}

void SerialPlot::init(UART_HandleTypeDef *_huart, PID *_single_loop)
{
	if (_huart->Instance == USART1) {
		uart_manager = &uart1_manager;
	} else if (_huart->Instance == USART2) {
		uart_manager = &uart2_manager;
	} else if (_huart->Instance == USART3) {
		uart_manager = &uart3_manager;
	} else if (_huart->Instance == UART4) {
		uart_manager = &uart4_manager;
	} else if (_huart->Instance == UART5) {
		uart_manager = &uart5_manager;
	} else if (_huart->Instance == USART6) {
		uart_manager = &uart6_manager;
	}
	single_loop = _single_loop;
	mode = SINGLE_MODE;
}

void SerialPlot::init(UART_HandleTypeDef *_huart, PID *_single_loop,
                      PID *_double_loop)
{
	if (_huart->Instance == USART1) {
		uart_manager = &uart1_manager;
	} else if (_huart->Instance == USART2) {
		uart_manager = &uart2_manager;
	} else if (_huart->Instance == USART3) {
		uart_manager = &uart3_manager;
	} else if (_huart->Instance == UART4) {
		uart_manager = &uart4_manager;
	} else if (_huart->Instance == UART5) {
		uart_manager = &uart5_manager;
	} else if (_huart->Instance == USART6) {
		uart_manager = &uart6_manager;
	}
	single_loop = _single_loop;
	double_loop = _double_loop;
	mode = DOUBLE_MODE;
}

void SerialPlot::print(const char *_format, ...)
{
	static uint8_t tx_buf[128] = {0};
	static va_list ap;
	static uint16_t len;
	va_start(ap, _format);
	len = vsprintf((char *)tx_buf, _format, ap);
	va_end(ap);

	uart_send_data(uart_manager->huart, tx_buf, len);
}

void SerialPlot::print_dma(const char *_format, ...)
{
	static uint8_t tx_buf[256] = {0};
	static va_list ap;
	static uint16_t len;
	va_start(ap, _format);
	len = vsprintf((char *)tx_buf, _format, ap);
	va_end(ap);

	uart_send_data_dma(uart_manager->huart, tx_buf, len);
}

void SerialPlot::vofa_just_float(const char *_format, ...)
{
	static uint8_t tx_buf[256];
	static float float_args[16];
	uint16_t num_floats = 0;

	va_list args;
	va_start(args, _format);

	// 遍历格式字符串，提取浮点数（类似 scanf 的逻辑）
	while (*_format != '\0') {
		if (*_format == '%') {
			_format++;              // 跳过 '%'
			if (*_format == 'f') {  // 只处理 %f
				float_args[num_floats++] =
				    (float)va_arg(args, double);  // float 会被提升为 double
				if (num_floats >= 16) break;      // 防止溢出
			}
		}
		_format++;
	}
	va_end(args);

	uint16_t data_len = num_floats * sizeof(float);
	memcpy(tx_buf, float_args, data_len);

	const uint8_t tail[4] = {0x00, 0x00, 0x80, 0x7F};
	memcpy(tx_buf + data_len, tail, sizeof(tail));

	uart_send_data(uart_manager->huart, tx_buf, data_len + sizeof(tail));
}

void SerialPlot::vofa_just_float_dma(const char *_format, ...)
{
	static uint8_t tx_buf[256];
	static float float_args[16];
	uint16_t num_floats = 0;

	va_list args;
	va_start(args, _format);

	// 遍历格式字符串，提取浮点数（类似 scanf 的逻辑）
	while (*_format != '\0') {
		if (*_format == '%') {
			_format++;              // 跳过 '%'
			if (*_format == 'f') {  // 只处理 %f
				float_args[num_floats++] =
				    (float)va_arg(args, double);  // float 会被提升为 double
				if (num_floats >= 16) break;      // 防止溢出
			}
		}
		_format++;
	}
	va_end(args);

	uint16_t data_len = num_floats * sizeof(float);
	memcpy(tx_buf, float_args, data_len);

	const uint8_t tail[4] = {0x00, 0x00, 0x80, 0x7F};
	memcpy(tx_buf + data_len, tail, sizeof(tail));

	uart_send_data_dma(uart_manager->huart, tx_buf, data_len + sizeof(tail));
}

void SerialPlot::pid_adjust()
{
	float value;
	if (mode == SINGLE_MODE) {
		switch (uart_manager->rx_buffer[0]) {
			case (SINGLE_KP): {
				value = bytes_to_float(uart_manager->rx_buffer, 1);
				single_loop->set_kp(value);
			} break;
			case (SINGLE_KI): {
				value = bytes_to_float(uart_manager->rx_buffer, 1);
				single_loop->set_ki(value);
			} break;
			case (SINGLE_KD): {
				value = bytes_to_float(uart_manager->rx_buffer, 1);
				single_loop->set_kd(value);
			} break;
			case (SINGLE_KF): {
				value = bytes_to_float(uart_manager->rx_buffer, 1);
				single_loop->set_kf(value);
			} break;
			case (SINGLE_TARGET): {
				value = bytes_to_float(uart_manager->rx_buffer, 1);
				single_loop->set_target(value);
			} break;
		}
	} else if (mode == DOUBLE_MODE) {
		switch (uart_manager->rx_buffer[0]) {
			case (SINGLE_KP): {
				value = bytes_to_float(uart_manager->rx_buffer, 1);
				single_loop->set_kp(value);
			} break;
			case (SINGLE_KI): {
				value = bytes_to_float(uart_manager->rx_buffer, 1);
				single_loop->set_ki(value);
			} break;
			case (SINGLE_KD): {
				value = bytes_to_float(uart_manager->rx_buffer, 1);
				single_loop->set_kd(value);
			} break;
			case (SINGLE_KF): {
				value = bytes_to_float(uart_manager->rx_buffer, 1);
				single_loop->set_kf(value);
			} break;
			case (SINGLE_TARGET): {
				value = bytes_to_float(uart_manager->rx_buffer, 1);
				single_loop->set_target(value);
			} break;
			case (DOUBLE_KP): {
				value = bytes_to_float(uart_manager->rx_buffer, 1);
				double_loop->set_kp(value);
			} break;
			case (DOUBLE_KI): {
				value = bytes_to_float(uart_manager->rx_buffer, 1);
				double_loop->set_ki(value);
			} break;
			case (DOUBLE_KD): {
				value = bytes_to_float(uart_manager->rx_buffer, 1);
				double_loop->set_kd(value);
			} break;
			case (DOUBLE_KF): {
				value = bytes_to_float(uart_manager->rx_buffer, 1);
				double_loop->set_kf(value);
			} break;
			case (DOUBLE_TARGET): {
				value = bytes_to_float(uart_manager->rx_buffer, 1);
				double_loop->set_target(value);
			} break;
		}
	}
}

void SerialPlot::judge_stop()
{
	if (uart_manager->rx_buffer[0] == PLOT_STOP &&
	    uart_manager->rx_buffer[1] == PLOT_STOP &&
	    uart_manager->rx_buffer[2] == PLOT_STOP &&
	    uart_manager->rx_buffer[3] == PLOT_STOP) {
		pid_clear();
	}
}

void SerialPlot::pid_clear()
{
	if (mode == SINGLE_MODE) {
		single_loop->set_target(0);
		single_loop->set_kp(0);
		single_loop->set_ki(0);
		single_loop->set_kd(0);
		single_loop->set_kf(0);
	} else if (mode == DOUBLE_MODE) {
		single_loop->set_target(0);
		single_loop->set_kp(0);
		single_loop->set_ki(0);
		single_loop->set_kd(0);
		single_loop->set_kf(0);
		double_loop->set_target(0);
		double_loop->set_kp(0);
		double_loop->set_ki(0);
		double_loop->set_kd(0);
		double_loop->set_kf(0);
	}
}

void SerialPlot::data_pross()
{
	judge_stop();
	pid_adjust();
}

void SerialPlot::vofa_show_pid_loop()
{
	if (mode == SINGLE_MODE) {
		vofa_just_float_dma("%f%f%f%f%f%f%f", single_loop->get_target(),
		                    single_loop->get_ref(), single_loop->get_out(),
		                    single_loop->get_kp(), single_loop->get_ki(),
		                    single_loop->get_kd(), single_loop->get_kf());
	} else if (mode == DOUBLE_MODE) {
		vofa_just_float_dma("%f%f%f%f%f%f%f%f%f%f%f%f%f%f",
		                    single_loop->get_target(), single_loop->get_ref(),
		                    single_loop->get_out(), single_loop->get_kp(),
		                    single_loop->get_ki(), single_loop->get_kd(),
		                    single_loop->get_kf(), double_loop->get_target(),
		                    double_loop->get_ref(), double_loop->get_out(),
		                    double_loop->get_kp(), double_loop->get_ki(),
		                    double_loop->get_kd(), double_loop->get_kf());
	}
}

void SerialPlot::UART_RxCallback(uint8_t *_rx_data) { data_pross(); }

/*========================COPYRIGHT(C) CCUT-DAMOCLES==========================*/
